#include "bsp/cpu_ticks.h"
#include "controller/motor.h"
#include "utils/utils.h"
#include "utils/logger.h"

motor_t g_motor[1];

void motor_init(motor_t *motor){
    memset(motor, 0, sizeof(motor_t));
    mc_conf_init();
	motor_init_encoder(motor);
	pwm_timer_init(mc_conf()->pwm_freq);
	hw_sample_init(&motor->samples);
	contrl_init(controller(motor));
	pwm_enable_update_irq(true);
    mc_sched_timer_init(freq_2_us(CONFIG_SPEED_CTRL_FREQ));
	sys_debug("motor_init\n");
}

bool motor_start(motor_t *motor, u8 mode) {
	if (motor->b_start) {
        return true;
    }
	sys_debug("motor start\n");
	u32 mask = cpu_enter_critical();
	pwm_enable_update_irq(false);
	contrl_start(controller(motor), mode);
	motor->b_start = true;
	cpu_exit_critical(mask);
	hw_sample_get_phacurr_offset(&motor->samples);
	pwm_start_output();
    adc_start_convert();
	hw_sample_wait_phacurr_offset(&motor->samples);
	if (hw_sample_check_phacurr_offset(&motor->samples)){
		motor_stop(motor);
		return false;
	}
    return true;
}

bool motor_stop(motor_t *motor) {
    if (!motor->b_start) {
        return true;
    }
	sys_debug("motor stop\n");
    u32 mask = cpu_enter_critical();
    adc_stop_convert();
    pwm_stop_output();
    cpu_exit_critical(mask);
    cpu_udelay(100); /* give the time for pending adc irq finished*/
    mask = cpu_enter_critical();
    contrl_stop(controller(motor));
	motor->b_start = false;
    cpu_exit_critical(mask);
    pwm_enable_update_irq(true);
    return true;
}

__STATIC_INLINE void motor_update_samples(motor_t *motor) {
	motor_update_encoder(motor);
	hw_sample_get_phacurr(&motor->samples, motor->controller.phase_curr);
	motor->controller.encoder_angle = motor_get_encoder_angle(motor);    
    motor->controller.encoder_velocity = motor_get_encoder_velocity(motor);
	motor->controller.vbus_vol_sampled = hw_sample_get_vbus_vol(&motor->samples);
	motor->controller.vbus_curr_sampled = hw_sample_get_vbus_curr(&motor->samples);
}

void timer_up_irq_handler(motor_t *motor){
	if (!motor->b_start) {
		motor_update_samples(motor);
	}
}

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void motor_adc_irq_handler(motor_t *motor) {
	controller_t *ctrl = controller(motor);
	foc_t *foc = &ctrl->foc;
	if (hw_sample_calc_phacurr_offset(&motor->samples)) {
		return;
	}
	motor_update_samples(motor);
	contrl_get_phase_duty(ctrl);
	u16 ta = foc->duty_norm[0] * mc_conf()->pwm_half_period;
    u16 tb = foc->duty_norm[1] * mc_conf()->pwm_half_period;
    u16 tc = foc->duty_norm[2] * mc_conf()->pwm_half_period;
    pwm_apply_duty(ta, tb, tc);
}

/*slow的部分处理，比如速度环，状态机*/
void mc_sched_irq_handler(motor_t *motor) {
	motor->mode_running = contrl_update_mode(controller(motor));
	if (motor->mode_running == CTRL_MODE_TORQUE) {
		contrl_set_target_torque(controller(motor), motor->torque_command);
	}else if (motor->mode_running == CTRL_MODE_VELOCITY) {
		contrl_set_target_velocity(controller(motor), motor->velocity_command);
	}else if (motor->b_start && motor->mode_running == CTRL_MODE_OPEN) {
		contrl_set_target_vdq(controller(motor), motor->vdq_command[0], motor->vdq_command[1]);
	}

	contrl_update_target(controller(motor));
}
